/*
 * @FileName: loop.h
 * @Author: GreyQiu qiushaogui@aikosolar.com
 * @Date: 2023-07-12 16:05:59
 * @LastEditors: GreyQiu qiushaogui@aikosolar.com
 * @LastEditTime: 2023-09-14 11:10:42
 * @Description: 
 * 
 * Copyright (c) 2023, All Rights Reserved. 
 */

#ifndef __LOOP_H__
#define __LOOP_H__

#include "bsp.h"
#include "easy_pid_f32.h"
#include "easy_filter.h"
#include "bsp_pwm.h"
#include "dev_info.h"
#include "app_mppt.h"
typedef struct loop
{
    uint8_t enable;
    uint8_t mux;
    float Vset;
    float Iset;
    float VsetDischarge;
} loop_status_t;


#define TS  (BSP_PWM_PERIOD / 2) // 环路2个开关周期运算一次

/**
 * @description: 电压环参数
 */
#define VOUT_PID_KP  (2.0f)
#define VOUT_PID_KI  (1257.0f)
#define VOUT_PID_KD  (0.0f)

/**
 * @description: 电流环参数
 */
//#define IOUT_PID_KP  (0.01f)
//#define IOUT_PID_KI  (628.3f)
#define IOUT_PID_KP  (0.01f)
#define IOUT_PID_KI  (188.5f)

/**
 * @description: 输入环参数
 */
#define VIN_PID_KP  (0.0f)
#define VIN_PID_KI  (-25.0f)

/**
 * @description: 补偿器积分钳位值
 */
#define VLOOP_OUT_MAX    (3.0f)
#define VLOOP_OUT_MIN    (-3.0f)

#define ILOOP_OUT_MAX    (1.95f)
#define ILOOP_OUT_MIN    (0.0f)

#define DUTY_MIN    (0.08f)

typedef enum {
    MUX_NONE = 0, // 无环路竞争成功，所有环路饱和
    MUX_VLOOP,
    MUX_ILOOP,
    MUX_GLOOP
} mux_t;


extern easy_pid_f32_t vout_loop; // 输出电压 PID控制
extern easy_pi_f32_t  iout_loop; // 输出电流 PI控制
extern easy_pid_f32_t vin_loop; // 输入电压 积分控制

extern loop_status_t loop_status;
extern easy_lpf_f32_t duty_flt;
extern uint16_t pwr_dir;
extern uint16_t fsbbMode;

/**
 * @description: 正向功率环路
 * @return 无
 */
static inline void fsbb_loop_forward_run(float vout_fb, float iout_fb, float vin_fb, float iin_fb, float *d) {
    float out;

    iout_loop.ref = easy_pid_f32(&vout_loop, vout_fb);
    out = easy_pi_f32(&iout_loop, iout_fb);
    easy_lpf_f32(&duty_flt, out); // 占空比低通滤波

    // 注意！！！ 占空比赋值不可以放在范围判断里执行，否则当OUT在回差盲区内，占空比不更新！！
    // 正确做法为，根据OUT范围切换FSBB的发波模式，然后再根据发波模式更新占空比，这样就不会受盲区影响不更新。
//    if (out <= 0.9f) {
//        fsbbMode = 0;
////        d[0] = out;
////        d[1] = 0.0f;
//    } else if (out > 0.95f && out <= 1.0f) {
//        fsbbMode = 1;
////        d[0] = out * 0.95f;
////        d[1] = 0.05f;
//    } else if (out > 1.0f && out <= 1.05f) {
//        fsbbMode = 2;
////        d[0] = 0.95f;
////        d[1] = 1 - (2.0f - out) * 0.95f;
//    } else if (out > 1.1f){
//        fsbbMode = 3;
////        d[0] = 1.0f;
////        d[1] = out - 1.0f;
//    }

    switch (fsbbMode) {
        case 0:
            d[0] = out;
            d[1] = 0.0f;
            break;
        case 1:
            d[0] = out * 0.95f;
            d[1] = 0.05f;
            break;
        case 2:
            d[0] = 0.95f;
            d[1] = 1 - (2.0f - out) * 0.95f;
            break;
        case 3:
            d[0] = 1.0f;
            d[1] = out - 1.0f;
            break;
        default: break;
    }
}

/**
 * @description: 反向功率环路
 * @return 无
 */
static inline void fsbb_loop_invert_run(float vout_fb, float iout_fb, float vin_fb, float iin_fb, float *d) {
    float out;

    iout_loop.ref = easy_pid_f32(&vout_loop, vout_fb);
    out = easy_pi_f32(&iout_loop, -iout_fb);
    easy_lpf_f32(&duty_flt, -out); // 占空比低通滤波
    // 注意！！！ 占空比赋值不可以放在范围判断里执行，否则当OUT在回差盲区内，占空比不更新！！
    // 正确做法为，根据OUT范围切换FSBB的发波模式，然后再根据发波模式更新占空比，这样就不会受盲区影响不更新。
//    if (out <= 0.9f) {
//        fsbbMode = 0;
////        d[0] = out;
////        d[1] = 0.0f;
//    } else if (out > 0.95f && out <= 1.0f) {
//        fsbbMode = 1;
////        d[0] = out * 0.95f;
////        d[1] = 0.05f;
//    } else if (out > 1.0f && out <= 1.05f) {
//        fsbbMode = 2;
////        d[0] = 0.95f;
////        d[1] = 1 - (2.0f - out) * 0.95f;
//    } else if (out > 1.1f){
//        fsbbMode = 3;
////        d[0] = 1.0f;
////        d[1] = out - 1.0f;
//    }

    switch (fsbbMode) {
        case 0:
            d[1] = out;
            d[0] = 0.0f;
            break;
        case 1:
            d[1] = out * 0.95f;
            d[0] = 0.05f;
            break;
        case 2:
            d[1] = 0.95f;
            d[0] = 1 - (2.0f - out) * 0.95f;
            break;
        case 3:
            d[1] = 1.0f;
            d[0] = out - 1.0f;
            break;
        default: break;
    }
}

static inline void fsbb_loop_run(float vout_fb, float iout_fb, float vin_fb, float iin_fb, float *d)
{
    if (pwr_dir == 2) {
        fsbb_loop_forward_run(vout_fb, iout_fb, vin_fb, iin_fb, d);
    } else if (pwr_dir == 1) {
        fsbb_loop_invert_run(vout_fb, iout_fb, vin_fb, iin_fb, d);
    }
}

/**
 * @description: 设置电压环基准
 * @param {float} ref 基准值
 * @return 无
 */
static inline void vout_loop_ref_set(float ref)
{
    vout_loop.ref = ref;
}

/**
 * @description: 获取电压环基准
 * @return 无
 */
static inline float vout_loop_ref_get(void)
{
    return vout_loop.ref;
}

/**
 * @description: 设置电流环基准
 * @param {float} ref 基准值
 * @return 无
 */
static inline void iout_loop_ref_set(float ref)
{
    iout_loop.ref = ref;
}

/**
 * @description: 获取电流环基准
 * @return 无
 */
static inline float iout_loop_ref_get(void)
{
    return iout_loop.ref;
}

/**
 * @description: 设置输入环基准
 * @param {float} ref 基准值
 * @return 无
 */
static inline void vin_loop_ref_set(float ref)
{
    vin_loop.ref = ref;
}

/**
 * @description: 获取输入环基准
 * @return 无
 */
static inline float vin_loop_ref_get(void)
{
    return vin_loop.ref;
}

/**
 * @description: 电压环清0
 * @return 无
 */
static inline void vout_loop_clear(void)
{
    vout_loop.Yn_1 = 0;
    vout_loop.Xn_1 = 0;
    vout_loop.Xn_2 = 0;
}

/**
 * @description: 电流环清0
 * @return 无
 */
static inline void iout_loop_clear(void)
{
    iout_loop.Yn_1 = 0;
    iout_loop.Xn_1 = 0;
}

/**
 * @description: 输入环清0
 * @return 无
 */
static inline void vin_loop_clear(void)
{
    vin_loop.Yn_1 = 0;
}

/**
 * @description: 查询环路是否使能
 * @return 无
 */
static inline bool loop_is_enable(void)
{
    return loop_status.enable == ENABLE;
}

void loop_init(void);
void loop_enable(void);
void loop_disable(void);

bool vout_loop_saturation(void);
bool iout_loop_saturation(void);
bool vin_loop_saturation(void);
float loop_get_duty(void);
void loop_task(void);
#endif // __LOOP_H__

